/*    

    bobbietalk - a visual programming environment for toy robotics

    Copyright (C) 2013  Roberto Tiella

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
package bobbietalk.simulator;

import java.util.Observer;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;

import org.apache.log4j.Level;
import org.apache.log4j.Logger;
import bobbietalk.simulator.world.CinematicWorldObject;

/**
 * A robot platform: it lacks the body
 *
 * motion model is based on two indipent motors, each one controlling a weel
 * each weel is connected to a ticker counter. when the body hits an obstacle
 * the robots goes in the opposite until it exit the collision and then it
 * stops.
 *
 * @author tiella
 */
public class SimpleMotorsPlatform extends CinematicWorldObject {

    public static final int TICKS_PER_TURN = 360;
    private Logger logger = Logger.getLogger(this.getClass());
    private double leftMotor = 0;
    private double rightMotor = 0;
    private AbstractSimpleRobot robot;
//    private Vector3d speed = new Vector3d(0, 0, 0);
//    private Matrix3d rotation = new Matrix3d(one);
    boolean pause = false;
    private double tickl = 0;
    private double tickr = 0;
    private int hits = 0;
    private int power = 0; // power consuption
    private static int REPORT_POWER_PERIOD = 10;
    private int reportPowerCounter = REPORT_POWER_PERIOD;
    private double wd = 0.04; // wheel diameter
    private double rotationsPerSeconds = 0.875; // rotation per seconds
    private double leftDefinedSpeed;
    private double rightDefinedSpeed;
    private double vToTick = 0.05;
    private ObservableDelegate observable = new ObservableDelegate();

    /**
     * A simple model for a differential driving platform.
     * 
     * motors must be turned on with leftMotorForward,rightMotorForward,etc. in order to have the robot moving
     */
    public SimpleMotorsPlatform(AbstractSimpleRobot r) {
        robot = r;
        logger.setLevel(Level.DEBUG);
        leftDefinedSpeed = speedFromRotations(rotationsPerSeconds);
        rightDefinedSpeed = leftDefinedSpeed;
    }

    public void init() {
    }

    public synchronized void addObserver(Observer o) {
        observable.addObserver(o);
    }

    public synchronized void deleteObserver(Observer o) {
        observable.deleteObserver(o);
    }

    public void setMotorSpeeds(double leftRot, double rightRot) {

        // double oldLeftMotor = leftMotor;
        leftDefinedSpeed = speedFromRotations(leftRot);
        rightDefinedSpeed = speedFromRotations(rightRot);


    }

    public void setLeftMotorSpeed(double rot) {


        // double oldLeftMotor = leftMotor;
        leftDefinedSpeed = speedFromRotations(rot);


    }

    public void setRightMotorSpeed(double rot) {

        rightDefinedSpeed = speedFromRotations(rot);


    }

    public void leftMotorForward() {

        leftMotor = 1;


    }

    public void rightMotorForward() {

        rightMotor = 1;


    }

    public void leftMotorBackward() {

        leftMotor = -1;


    }

    public void rightMotorBackward() {

        rightMotor = -1;


    }

    public void leftMotorOff() {

        leftMotor = 0;


    }

    public void rightMotorOff() {

        rightMotor = 0;


    }

    /**
     * called to change speed attributes
     */
    public void updateSpeed() {

//        double dt = 1.0 * delta / 1000.0;

        // double s = definedSpeed;  // tangential weel speed m/s

        double l = 0.17;   // weels separation (meters)
        double e = 0; // max absolute error on weel speed

        double vr = rightMotor * rightDefinedSpeed;
        double vl = leftMotor * leftDefinedSpeed;


//        vr += vr * e * (2 * Math.random() - 1);
//        vl += vl * e * (2 * Math.random() - 1);

        Vector3d speed = getTranslationSpeed();
        Matrix3d rotSpeed = getRotationSpeed();

//        if (vr == vl) {
        double omega = (- vl + vr) / l;
//            double r = (vr + vl) * l / (vr - vl);
        double v = (vl + vr) / 2;
        logger.debug("omega:" + omega + " v:" + v);

        Vector3d or = getOrientation();

        logger.debug("orient:" + or);

        speed.set(or.x * v, or.y * v, 0);
        rotSpeed.rotZ(omega * getDt());

//        } 
//        else {
//            Vector3d or = new Vector3d(getOrientation());
//            speed.set(or.x * vr, or.y * vr, 0); // vr == vl
//            rotSpeed.set(one);
//        }

        logger.debug("updated speed:" + speed + " rotation:" + rotSpeed);
    }

    public int getTickR() {
        return (int) tickr;
    }

    public int getTickL() {
        return (int) tickl;
    }

    public int getPower() {
        return power;
    }

    public double getAngle() {

        return Math.atan2(getOrientation().y, getOrientation().x);

    }

    public void notifyObservers() {

        observable.notifyObservers();
    }

    public void markChanged() {
        observable.markChanged();
    }

    private double speedFromRotations(double r) {
        return r * Math.PI * wd;
    }

    private double rotationsFromSpeed(double s) {
        return s / (Math.PI * wd);
    }

    @Override
    protected void positionUpdated(Vector3d oldPos, Matrix3d oldRot, Vector3d position, Matrix3d r) {
        
        tickr += rightMotor * (int) (TICKS_PER_TURN * rotationsFromSpeed(rightDefinedSpeed) * getDt());
        tickl += leftMotor * (int) (TICKS_PER_TURN * rotationsFromSpeed(leftDefinedSpeed) * getDt());

        // TODO maybe we can detect if the robot is blocked
    }
}
